//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>
#include <_context.h>
#include <init.h>
#include <entry.h>

uint32_t    g_kernelStack;

//
// ThreadHContext
//
void ThreadHContext::SaveFPU()
{
#ifndef _NOFLOAT
    if (m_uFlags & ThreadHContextFlag_FPUUsed) {
#ifdef _DEBUG
        uint32_t    uStatus;

        read_cp0_register($12, uStatus);
        assert(!(uStatus & ST0_FR));
        assert(uStatus & ST0_CU1);
#endif

#if defined(_GNUC)
        ASM("cfc1       %0, $31;":"=&r"(m_fpuContext.status));

        uint32_t    *puAddr = m_fpuContext.FGR;

        ASM("swc1       $f0,  0x00(%0);":"=&r"(puAddr));
        ASM("swc1       $f1,  0x04(%0);":"=&r"(puAddr));
        ASM("swc1       $f2,  0x08(%0);":"=&r"(puAddr));
        ASM("swc1       $f3,  0x0c(%0);":"=&r"(puAddr));
        ASM("swc1       $f4,  0x10(%0);":"=&r"(puAddr));
        ASM("swc1       $f5,  0x14(%0);":"=&r"(puAddr));
        ASM("swc1       $f6,  0x18(%0);":"=&r"(puAddr));
        ASM("swc1       $f7,  0x1c(%0);":"=&r"(puAddr));
        ASM("swc1       $f8,  0x20(%0);":"=&r"(puAddr));
        ASM("swc1       $f9,  0x24(%0);":"=&r"(puAddr));
        ASM("swc1       $f10, 0x28(%0);":"=&r"(puAddr));
        ASM("swc1       $f11, 0x2c(%0);":"=&r"(puAddr));
        ASM("swc1       $f12, 0x30(%0);":"=&r"(puAddr));
        ASM("swc1       $f13, 0x34(%0);":"=&r"(puAddr));
        ASM("swc1       $f14, 0x38(%0);":"=&r"(puAddr));
        ASM("swc1       $f15, 0x3c(%0);":"=&r"(puAddr));
        ASM("swc1       $f16, 0x40(%0);":"=&r"(puAddr));
        ASM("swc1       $f17, 0x44(%0);":"=&r"(puAddr));
        ASM("swc1       $f18, 0x48(%0);":"=&r"(puAddr));
        ASM("swc1       $f19, 0x4c(%0);":"=&r"(puAddr));
        ASM("swc1       $f20, 0x50(%0);":"=&r"(puAddr));
        ASM("swc1       $f21, 0x54(%0);":"=&r"(puAddr));
        ASM("swc1       $f22, 0x58(%0);":"=&r"(puAddr));
        ASM("swc1       $f23, 0x5c(%0);":"=&r"(puAddr));
        ASM("swc1       $f24, 0x60(%0);":"=&r"(puAddr));
        ASM("swc1       $f25, 0x64(%0);":"=&r"(puAddr));
        ASM("swc1       $f26, 0x68(%0);":"=&r"(puAddr));
        ASM("swc1       $f27, 0x6c(%0);":"=&r"(puAddr));
        ASM("swc1       $f28, 0x70(%0);":"=&r"(puAddr));
        ASM("swc1       $f29, 0x74(%0);":"=&r"(puAddr));
        ASM("swc1       $f30, 0x78(%0);":"=&r"(puAddr));
        ASM("swc1       $f31, 0x7c(%0);":"=&r"(puAddr));
#elif defined(_EVC)
        ASM(
            "cfc1       $12, $31;"
            "sw         $12, 0(%0);", &m_fpuContext.status);

        uint32_t    *puAddr = m_fpuContext.FGR;

        ASM("swc1       $f0,  0x00(%0);", puAddr);
        ASM("swc1       $f1,  0x04(%0);", puAddr);
        ASM("swc1       $f2,  0x08(%0);", puAddr);
        ASM("swc1       $f3,  0x0c(%0);", puAddr);
        ASM("swc1       $f4,  0x10(%0);", puAddr);
        ASM("swc1       $f5,  0x14(%0);", puAddr);
        ASM("swc1       $f6,  0x18(%0);", puAddr);
        ASM("swc1       $f7,  0x1c(%0);", puAddr);
        ASM("swc1       $f8,  0x20(%0);", puAddr);
        ASM("swc1       $f9,  0x24(%0);", puAddr);
        ASM("swc1       $f10, 0x28(%0);", puAddr);
        ASM("swc1       $f11, 0x2c(%0);", puAddr);
        ASM("swc1       $f12, 0x30(%0);", puAddr);
        ASM("swc1       $f13, 0x34(%0);", puAddr);
        ASM("swc1       $f14, 0x38(%0);", puAddr);
        ASM("swc1       $f15, 0x3c(%0);", puAddr);
        ASM("swc1       $f16, 0x40(%0);", puAddr);
        ASM("swc1       $f17, 0x44(%0);", puAddr);
        ASM("swc1       $f18, 0x48(%0);", puAddr);
        ASM("swc1       $f19, 0x4c(%0);", puAddr);
        ASM("swc1       $f20, 0x50(%0);", puAddr);
        ASM("swc1       $f21, 0x54(%0);", puAddr);
        ASM("swc1       $f22, 0x58(%0);", puAddr);
        ASM("swc1       $f23, 0x5c(%0);", puAddr);
        ASM("swc1       $f24, 0x60(%0);", puAddr);
        ASM("swc1       $f25, 0x64(%0);", puAddr);
        ASM("swc1       $f26, 0x68(%0);", puAddr);
        ASM("swc1       $f27, 0x6c(%0);", puAddr);
        ASM("swc1       $f28, 0x70(%0);", puAddr);
        ASM("swc1       $f29, 0x74(%0);", puAddr);
        ASM("swc1       $f30, 0x78(%0);", puAddr);
        ASM("swc1       $f31, 0x7c(%0);", puAddr);
#else
#error unknown compiler
#endif
        m_uFlags &= ~ThreadHContextFlag_FPUUsed;
        DisableCP1();
    }
#endif  // _NOFLOAT
}

void ThreadHContext::RestoreFPU(InterruptContext *pContext)
{
#ifdef _NOFLOAT
    ASM("break 0;");
#else

#ifdef _DEBUG
    uint32_t    uStatus;

    read_cp0_register($12, uStatus);
    assert(!(uStatus & ST0_FR));
    assert(uStatus & ST0_CU1);
#endif

    // Enable FPU, when return to user mode
    assert(!(pContext->cp0_status & ST0_CU1));

    if (m_uFlags & ThreadHContextFlag_FPUInited) {
        // clean the pipeline
#if defined(_GNUC)
        ASM("cfc1       $0, $31;"
            "ctc1       $12, $31;"::"r"(m_fpuContext.status));

        uint32_t        *puAddr = m_fpuContext.FGR;

        ASM("lwc1       $f0,  0x00(%0);"::"r"(puAddr));
        ASM("lwc1       $f1,  0x04(%0);"::"r"(puAddr));
        ASM("lwc1       $f2,  0x08(%0);"::"r"(puAddr));
        ASM("lwc1       $f3,  0x0c(%0);"::"r"(puAddr));
        ASM("lwc1       $f4,  0x10(%0);"::"r"(puAddr));
        ASM("lwc1       $f5,  0x14(%0);"::"r"(puAddr));
        ASM("lwc1       $f6,  0x18(%0);"::"r"(puAddr));
        ASM("lwc1       $f7,  0x1c(%0);"::"r"(puAddr));
        ASM("lwc1       $f8,  0x20(%0);"::"r"(puAddr));
        ASM("lwc1       $f9,  0x24(%0);"::"r"(puAddr));
        ASM("lwc1       $f10, 0x28(%0);"::"r"(puAddr));
        ASM("lwc1       $f11, 0x2c(%0);"::"r"(puAddr));
        ASM("lwc1       $f12, 0x30(%0);"::"r"(puAddr));
        ASM("lwc1       $f13, 0x34(%0);"::"r"(puAddr));
        ASM("lwc1       $f14, 0x38(%0);"::"r"(puAddr));
        ASM("lwc1       $f15, 0x3c(%0);"::"r"(puAddr));
        ASM("lwc1       $f16, 0x40(%0);"::"r"(puAddr));
        ASM("lwc1       $f17, 0x44(%0);"::"r"(puAddr));
        ASM("lwc1       $f18, 0x48(%0);"::"r"(puAddr));
        ASM("lwc1       $f19, 0x4c(%0);"::"r"(puAddr));
        ASM("lwc1       $f20, 0x50(%0);"::"r"(puAddr));
        ASM("lwc1       $f21, 0x54(%0);"::"r"(puAddr));
        ASM("lwc1       $f22, 0x58(%0);"::"r"(puAddr));
        ASM("lwc1       $f23, 0x5c(%0);"::"r"(puAddr));
        ASM("lwc1       $f24, 0x60(%0);"::"r"(puAddr));
        ASM("lwc1       $f25, 0x64(%0);"::"r"(puAddr));
        ASM("lwc1       $f26, 0x68(%0);"::"r"(puAddr));
        ASM("lwc1       $f27, 0x6c(%0);"::"r"(puAddr));
        ASM("lwc1       $f28, 0x70(%0);"::"r"(puAddr));
        ASM("lwc1       $f29, 0x74(%0);"::"r"(puAddr));
        ASM("lwc1       $f30, 0x78(%0);"::"r"(puAddr));
        ASM("lwc1       $f31, 0x7c(%0);"::"r"(puAddr));
#elif defined(_EVC)
        ASM("cfc1       $0, $31;"
            "move       $12, %0;"
            "ctc1       $12, $31;", m_fpuContext.status);

        uint32_t        *puAddr = m_fpuContext.FGR;

        ASM("lwc1       $f0,  0x00(%0);", puAddr);
        ASM("lwc1       $f1,  0x04(%0);", puAddr);
        ASM("lwc1       $f2,  0x08(%0);", puAddr);
        ASM("lwc1       $f3,  0x0c(%0);", puAddr);
        ASM("lwc1       $f4,  0x10(%0);", puAddr);
        ASM("lwc1       $f5,  0x14(%0);", puAddr);
        ASM("lwc1       $f6,  0x18(%0);", puAddr);
        ASM("lwc1       $f7,  0x1c(%0);", puAddr);
        ASM("lwc1       $f8,  0x20(%0);", puAddr);
        ASM("lwc1       $f9,  0x24(%0);", puAddr);
        ASM("lwc1       $f10, 0x28(%0);", puAddr);
        ASM("lwc1       $f11, 0x2c(%0);", puAddr);
        ASM("lwc1       $f12, 0x30(%0);", puAddr);
        ASM("lwc1       $f13, 0x34(%0);", puAddr);
        ASM("lwc1       $f14, 0x38(%0);", puAddr);
        ASM("lwc1       $f15, 0x3c(%0);", puAddr);
        ASM("lwc1       $f16, 0x40(%0);", puAddr);
        ASM("lwc1       $f17, 0x44(%0);", puAddr);
        ASM("lwc1       $f18, 0x48(%0);", puAddr);
        ASM("lwc1       $f19, 0x4c(%0);", puAddr);
        ASM("lwc1       $f20, 0x50(%0);", puAddr);
        ASM("lwc1       $f21, 0x54(%0);", puAddr);
        ASM("lwc1       $f22, 0x58(%0);", puAddr);
        ASM("lwc1       $f23, 0x5c(%0);", puAddr);
        ASM("lwc1       $f24, 0x60(%0);", puAddr);
        ASM("lwc1       $f25, 0x64(%0);", puAddr);
        ASM("lwc1       $f26, 0x68(%0);", puAddr);
        ASM("lwc1       $f27, 0x6c(%0);", puAddr);
        ASM("lwc1       $f28, 0x70(%0);", puAddr);
        ASM("lwc1       $f29, 0x74(%0);", puAddr);
        ASM("lwc1       $f30, 0x78(%0);", puAddr);
        ASM("lwc1       $f31, 0x7c(%0);", puAddr);
#else
#error unknown compiler
#endif
    }
    else {
#if defined(_GNUC)
        ASM(
            "cfc1   $0, $31;"
            "ctc1   %0, $31;"::"r"(CP1_STATUS));
#elif defined(_EVC)
        // attention: now, CP1_STATUS == 0
        ASM(
            "cfc1   $0, $31;"
            "ctc1   $0, $31;");
#else
#error unknown compiler
#endif
        m_fpuContext.status = 0;
        m_uFlags |= ThreadHContextFlag_FPUInited;
    }

    m_uFlags |= ThreadHContextFlag_FPUUsed;
#endif  // _NOFLOAT
}

//
// Global function definitions
//
typedef struct ThreadSwitchContext {
    uint32_t    uStatus;
    uint32_t    uRa;
    uint32_t    uS0, uS1, uS2, uS3, uS4, uS5, uS6, uS7, uS8;
} ThreadSwitchContext;

#define INIT_KERNELTHREAD_STATUS    0x00000001
#define INIT_USERTHREAD_STATUS      0x00000013

EXTERN virtaddr_t InitThread0Stack(Thread *pThread)
{
    return (virtaddr_t *)pThread->m_vaKernelStack;
}

//
// The layout of new internal kernel thread stack is:
//
// thread.m_vaKernelStack ->      |                      |
//                                |     Reserved         |
//                                +----------------------+
//                                |      s0 ~ s8         |
//                                |         ra           |
//                                |       status         |
//                                +----------------------+
//                                |     Reserved         |
//                                |----------------------|
// thread.m_vaKernelStackPtr ->   |    return address    | SystemThreadStartup
//                                +----------------------+
//

EXTERN_C void SystemThreadStartup();

EXTERN virtaddr_t InitSystemThreadStack(
    Thread *pThread, threadentry_t threadentry,
    threadroutine_t threadroutine, void *pvParameter)
{
    // Reserve
    pThread->m_vaKernelStack = (uint32_t *)pThread->m_vaKernelStack - 8;
    ThreadSwitchContext *pContext =
        (ThreadSwitchContext *)pThread->m_vaKernelStack - 1;

    pContext->uStatus = INIT_KERNELTHREAD_STATUS;
    pContext->uRa = (uint32_t)threadentry;
    pContext->uS5 = (uint32_t)threadroutine;
    pContext->uS6 = (uint32_t)pvParameter;

    // Reserve
    uint32_t *pCntxt = (uint32_t *)pContext - 4;

    *(pCntxt - 1) = (uint32_t)&SystemThreadStartup;

    return (virtaddr_t)(pCntxt - 1);
}

//
// The layout of new kernel thread stack is:
//
// thread.m_vaKernelStack ->      |                    |
//                                +--------------------+
//                                |      Reserved      |
//                                |--------------------|
//                                |                    |
//                                |  InterruptContext  |
//                                |                    |
//  pThread->m_vaKernelStack -->  +--------------------+
//                                |     Reserved       |
//                                |--------------------|
//                                |                    |
//                                |    switch stack    |
//                                |                    |
//                                +--------------------+
//                                |     Reserved       |
//                                +--------------------+
// thread.m_vaKernelStackPtr ->   |   return address   | ThreadStartup
//                                +--------------------+
//

EXTERN_C void ThreadStartup();

EXTERN virtaddr_t InitThreadStack(
    Thread *pThread, threadroutine_t threadroutine, void *pvParameter)
{
    // Reserve
    pThread->m_vaKernelStack = (uint32_t *)pThread->m_vaKernelStack - 8;

    InterruptContext *pIContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

    // Reserve
    uint32_t *pContxt = (uint32_t *)pIContext - 4;

    ThreadSwitchContext *pTSContext = (ThreadSwitchContext *)pContxt - 1;

    pTSContext->uStatus = INIT_KERNELTHREAD_STATUS;
    pTSContext->uRa = (uint32_t)threadroutine;
    pTSContext->uS5 = (uint32_t)pvParameter;

    // Reserve
    pContxt = (uint32_t *)pTSContext - 4;

    *(pContxt - 1) = (uint32_t)&ThreadStartup;

    return (virtaddr_t)(pContxt - 1);
}

#if defined(_GNUC)
EXTERN_C unsigned int _gp;
#elif defined(_EVC)

#else

#endif

EXTERN void SetThreadUserEntry(
    Thread *pThread, virtaddr_t vaUserStack, virtaddr_t vaUserEntry)
{
    InterruptContext *pIContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

#ifdef _DEBUG
    memset(pIContext, 0, sizeof(InterruptContext));
    pIContext->regs[RA] = 0xffffffff;
#endif
    pIContext->cp0_epc      = (uint32_t)vaUserEntry;
    pIContext->cp0_status   = INIT_USERTHREAD_STATUS;
    pIContext->regs[SP]       = (uint32_t)vaUserStack;

    assert(0x80000000 > (uint32_t)vaUserStack);
#if defined(_GNUC)
    pIContext->regs[GP]       = (uint32_t)&_gp;
#elif defined(_EVC)
    pIContext->regs[GP]       = 0;
#else
#error unknown compiler
#endif

    pIContext->regs[A0]       = *((uint32_t *)vaUserStack + 1);
    pIContext->regs[A1]       = *((uint32_t *)vaUserStack + 2);
    pIContext->regs[A2]       = *((uint32_t *)vaUserStack + 3);
    pIContext->regs[A3]       = *((uint32_t *)vaUserStack + 4);
}

//
// ProcessHContext
//
ECode ProcessHContext::SetBreakPoint(
    uint_t uNo, BreakPointType type, void *pvAddress, uint_t uLength)
{
#ifdef KCONFIG_GLOBAL_BREAKPOINT
    return E_NOT_IMPLEMENTED;
#else
    return E_NOT_IMPLEMENTED;
#endif // KCONFIG_GLOBAL_BREAKPOINT
}

#ifdef KCONFIG_GLOBAL_BREAKPOINT
EXTERN ECode SetGlobalBreakPoint(
    uint_t uNo, BreakPointType type, void *pvAddress, uint_t uLength)
{
    return E_NOT_IMPLEMENTED;
}
#endif // KCONFIG_GLOBAL_BREAKPOINT
